#include "visualizer.h"
#include <opencv2/highgui/highgui.hpp>

//#define USE_WINDOW

visualizer::visualizer(const char *name){
	this->window_name = name;
	initialColor();
}

void visualizer::initialColor(){

}

void visualizer::setBackground(int width, int height, unsigned char *r_map, unsigned char *g_map, unsigned char *b_map){
	/*fill in the color image*/
	src_img.create(height,width,CV_8UC3);


	int rv,gv,bv;
	int index;
	for( int r = 0; r<height; ++r){
		for( int c = 0; c<width; ++c){
			index = c*height+r;
			rv = r_map[index];
			gv = g_map[index];
			bv = b_map[index];
			src_img.at<cv::Vec3b>(r,c)[0]=bv;
			src_img.at<cv::Vec3b>(r,c)[1]=gv;
			src_img.at<cv::Vec3b>(r,c)[2]=rv;

			//src_img.setPixel(c,r,qRgb(rv,gv,bv));
		}
	}
	curr_img = src_img.clone();

#ifdef USE_WINDOW
	cvNamedWindow(window_name,CV_WINDOW_AUTOSIZE);
	cv::imshow(window_name,curr_img);
	cv::waitKey(1);
#endif

	// read data from file
	const char* file_name = "D:/Projects/AoT/testCode/detection/frame_pts_v2.csv";
	FILE *p_file = fopen(file_name,"r");
	if( p_file == NULL){
		const char* file_name_2 = "/home/wzhu/Documents/CarAoT/testCode/detection/frame_pts_v2.csv";
		p_file=fopen(file_name_2,"r");
		if(p_file==NULL){

			printf("Cannot open file : %s\n",file_name);
			return;
		}
	}
	// here we know that there are 28 points in the frame_pts.csv
	for(int i = 0; i<28; ++i){
		fscanf(p_file,"%f,%f,%f",&X[i][0],&X[i][1],&X[i][2]);
		//X[i][3]=1;
	}
	fclose(p_file);




}




void visualizer::drawLine(int x1, int y1, int x2, int y2, int color_index = 1, int thickness = 1){
	//if(x1<0||x1>curr_img.cols||x2<0||x2>curr_img.cols) {
	//	if(y1<0||y1>curr_img.rows||y2<0||y2>curr_img.rows) 
	//	return;
	//}

	int ic = color_index %80;
	cv::Point pt1, pt2;
	pt1.x = x1; pt1.y = y1;
	pt2.x = x2; pt2.y = y2;
	CV_RGB(255,0,0);
	/*add protection, if all line segments are out of bounary.*/
	cv::line(curr_img,pt1,pt2,CV_RGB(palette[ic][0],palette[ic][1],palette[ic][2]),2);

}

void visualizer::cleanImage(){
	curr_img =src_img.clone();
}

void visualizer::drawRootPtc(rootParticle_s &ptc){
	for(int i_vol =0; i_vol<ptc.vol_particle_list.size(); ++i_vol){
		this->drawVolPtc(ptc.vol_particle_list[i_vol],i_vol+20);
		char name[50];
		//itoa(ptc.vol_particle_list[i_vol].id,name,10);
		sprintf(&name[0],"%d",ptc.vol_particle_list[i_vol].id);
		this->drawText(0,(i_vol+1)*20, const_cast<const char*>(&name[0]),i_vol+20,2);
	}
}

void visualizer::drawVolPtc(volParticle_s &ptc, int i_color){
	for(int i_node =0; i_node<ptc.tan_particle_list.size(); ++i_node){
		this->drawLinePairs(ptc.tan_particle_list[i_node].pt_pairs,i_color,2);
	}
}

void visualizer::drawText(int x, int y, const char *txt, int color_index, int thickness){
	const std::string name(txt);
	cv::Point pt(x,y);
	int ic = color_index%80;
	cv::putText(curr_img,name,pt,cv::FONT_HERSHEY_PLAIN,.8,CV_RGB(palette[ic][0],palette[ic][1],palette[ic][2]),thickness);
}

void visualizer::drawLinePairs(std::deque<pt_pair_s> pts,int color_index = 7, int thickness =1){
	int npt = pts.size();
	cv::Point pt1, pt2;
	int c_row,c_col;
	cv::Size canvas_size = curr_img.size();
	c_row = canvas_size.height/2;
	c_col = canvas_size.width/2;


	int ic = color_index%80;
	for(int ipt= 0; ipt<npt; ipt+=2){
		pt1.x = pts[ipt][3]+c_col;
		pt1.y = pts[ipt][4]+c_row;

		pt2.x = pts[ipt+1][3]+c_col;
		pt2.y = pts[ipt+1][4]+c_row;

		cv::line(curr_img,pt1,pt2,CV_RGB(palette[ic][0],palette[ic][1],palette[ic][2]),thickness);
	}
	show(10);
}

void visualizer::show(int t=1){
#ifdef USE_WINDOW
	cv::imshow(window_name,curr_img);
	cv::waitKey(t);
#endif
}

void visualizer::save(int i = -1){
	return;
	char file_name[500];
	sprintf(&file_name[0],"%s/%04d.jpg","./car2",frame_index++);
	std::string sfile_name(file_name);
	cv::imwrite((sfile_name),curr_img);
}

void visualizer::drawP(float P[][4]){
	int sx = curr_img.rows;
	int sy = curr_img.cols;

	float x[16][3];

	// project the data points accordint to P
	for(int i = 0; i<16; ++i){
		x[i][0] = P[0][0]*X[i][0]+P[0][1]*X[i][1]+P[0][2]*X[i][2]+P[0][3];
		x[i][1] = P[1][0]*X[i][0]+P[1][1]*X[i][1]+P[1][2]*X[i][2]+P[1][3];
		x[i][2] = P[2][0]*X[i][0]+P[2][1]*X[i][1]+P[2][2]*X[i][2]+P[2][3];

		x[i][0] /=x[i][2];	x[i][1] /=x[i][2];

		x[i][0] = x[i][0] + sy/2;
		x[i][1] = x[i][1] + sx/2;
	}
	// show the points and lines in fixed order
	for(int i = 0; i<7; ++i){
		this->drawLine(x[i][0],x[i][1],x[i+1][0],x[i+1][1],0,1);
	}
	this->drawLine(x[7][0],x[7][1],x[0][0],x[0][1],0,2);



	for(int i = 8; i<15; ++i){
		this->drawLine(x[i][0],x[i][1],x[i+1][0],x[i+1][1],1,1);
	}
	this->drawLine(x[15][0],x[15][1],x[8][0],x[8][1],1,1);


	int s_pts[]={1,0,2,3,4,5,6,7};
	int e_pts[]={15,8,14,13,12,11,10,9};
	for(int i=0; i<4; ++i){
		this->drawLine(x[s_pts[i]][0],x[s_pts[i]][1],x[e_pts[i]][0],x[e_pts[i]][1],2,1);
	}
	for(int i=4; i<8; ++i){
		this->drawLine(x[s_pts[i]][0],x[s_pts[i]][1],x[e_pts[i]][0],x[e_pts[i]][1],2,2);
	}

	show();
	save();
}


void visualizer::drawTanTemplate(tanOR &tan, float P[3][4],float cam_dir[3],int color){
	tan.camera.updateCamera(&(P[0][0]),&cam_dir[0],1,1);
	if(!tan.camera.isVisible())
		return;
	line3D L;
	for( int i_line = 0; i_line<tan.tan_shape.line_list.size(); ++i_line){
		lineVertex my_line=tan.tan_shape.line_list[i_line];
		tan.camera.planePoint2World(my_line.x_start,my_line.y_start,L.x1,L.y1,L.z1);
		tan.camera.planePoint2World(my_line.x_end,my_line.y_end,L.x2,L.y2,L.z2);
		draw3DLine(P,L.x1,L.y1,L.z1,L.x2,L.y2,L.z2,color);
	}
	show();
}

void visualizer::draw3DLine(float P[3][4],float X1, float Y1, float Z1, float X2, float Y2, float Z2, int color){
	float u,v,w,u1,v1,u2,v2;
	float X,Y,Z;

	int sx = curr_img.rows;
	int sy = curr_img.cols;

	X = X1; Y = Y1; Z = Z1;
	u = P[0][0]*X+P[0][1]*Y+P[0][2]*Z+P[0][3];
	v = P[1][0]*X+P[1][1]*Y+P[1][2]*Z+P[1][3];
	w = P[2][0]*X+P[2][1]*Y+P[2][2]*Z+P[2][3];
	u1 = u/w+sy/2; v1=v/w+sx/2;

	X = X2; Y = Y2; Z = Z2;
	u = P[0][0]*X+P[0][1]*Y+P[0][2]*Z+P[0][3];
	v = P[1][0]*X+P[1][1]*Y+P[1][2]*Z+P[1][3];
	w = P[2][0]*X+P[2][1]*Y+P[2][2]*Z+P[2][3];
	u2 = u/w+sy/2; v2=v/w+sx/2;

	drawLine(u1,v1,u2,v2,color,4);
}
void visualizer::release(){
#ifdef USE_WINDOW
	cv::destroyWindow(this->window_name);
#endif
}

void visualizer::outImage(const char* file_name, int sx, int sy, float *map, int sub){

	float range = 100.0f;
	float min_val = -NEGMAX;
	float max_val = NEGMAX;
	/*initiate canvas, regulate, find mininum and maximum*/
	cv::Mat mat = cvCreateMat(sx/sub,sy/sub,CV_32FC1);
	
	for(int x = 0; x<sx; x+=sub){
		for(int y= 0; y<sy; y+=sub){
		int i = x + y*sx;
		float val = map[i];
		if(fabs(val)>range){
			val = 0;
		}else{
			min_val = std::min(min_val,val);
			max_val = std::max(max_val,val);
		}
		mat.at<float>(y/sub,x/sub)=val;
		}
	}
	range = max_val-min_val;
	for(int i = 0; i<sx*sy/sub/sub; ++i){
		mat.at<float>(i) = (mat.at<float>(i)-min_val)/range;
	}
	
	bool success = cv::imwrite(file_name,mat);
	if(!success){
		printf("failed to output image %s",file_name);
	}
	else{
		printf("image file exported to %s",file_name);
	}

	return;
}

void visualizer::outTanOrMap(int id, float pan, float tilt, float roll, float dist, int sx,int sy,float* map){
	char file_name[512];
	sprintf(&file_name[0]," id_%d_view_%d_%d_%d_%d.png",id,(int)floor(pan),(int)floor(tilt),(int)floor(roll),(int)floor(dist));
	outImage(const_cast<const char*>(&file_name[0]),sx,sy,map);
}
